%We can only find points in the camera's Pose
%Going from a point in camera pose to the same point in arm pose
%--  Pc = Tc2a * Pa
% => Pa = inv(Tc2a) * Pc

function a = cam2Arm_point(P_cam) 

%format the point given to this function
%=> 4:1 homogenous 
dimP = size(P_cam);

if  dimP(2) > 1
    P_cam = P_cam';
elseif dimP(1) < 4
    P_cam = [P_cam;1];
end 

%1) Have to define the pose of the arm relative
%   to the camera
tro_matrix=[0 1 0 0; 1 0 0 0; 0 0 -1 0; 0 0 0 1];
A_pose = transl(-0.18,0.08, 0)*tro_matrix;
      
%2)Transform points found in the camera pose
%  to points useable in the arms pose

%(just to make the logic easier)
%since the camera pose is the world pose,
%the Transform T_cam2arm == A_pose
T_ca = A_pose;

T_ac = inv(T_ca);

%note: P_cam is here put into homogeneous coordinates.
a = T_ac * P_cam;

end